// Copyright 2016 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <chrono>
#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "serial_helper.h"
#include <string>
#include <stdlib.h>
#include "yaogan.h"
using namespace std::chrono_literals;
using namespace std;
/* This example creates a subclass of Node and uses std::bind() to register a
 * member function as a callback from the timer. */

class MinimalPublisher : public rclcpp::Node
{
public:
  MinimalPublisher()
  : Node("yaokong_publisher"), count_(0)
  {
   // publisher_ = this->create_publisher<std_msgs::msg::String>("yaogan", 10);
   publisher_ = this->create_publisher<Yao_Gan>("yaogan", 10);
    timer_ = this->create_wall_timer(
      500ms, std::bind(&MinimalPublisher::timer_callback, this));
  }

private:
  void timer_callback()
  {
    auto message = std_msgs::msg::String();
    message.data = "Hello, world! " + std::to_string(count_++);
   // RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
   // publisher_->publish(message);
   publisher_->publish(yao_gan_info);
  }
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
  size_t count_;
  Yao_Gan yao_gan_info;
};
void readCallback(MsgBlock mb)
{
    std::cout << "recv: " << mb.toString() << std::endl;
    mb.vec[0]=0x12;
   
}
int main(int argc, char * argv[])
{
    int rtt;
    int16_t speed,sa,sb;
    SerialConfig sc;
    u_char *can_rec_str;
   std::vector<u8> uart_rec;
     u_char str_ch[50];
    //
    
    sc.baud = 115200;
    sc.port = "ttyS1";
    string ss = "hello";

    unsigned char sd[8];
    SerialHelper sh("chassis", sc);
    sh.setReadCallback(readCallback);
    sh.open();
    sh.run();

  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalPublisher>());
  rclcpp::shutdown();
  return 0;
}
